#!/usr/bin/env python
#-*- coding=UTF-8 -*-
import rospy
import math
import tf2_ros
import geometry_msgs.msg
import sys
import os

def mkdir(path):
    folder = os.path.exists(path)
    if not folder:
        os.system('mkdir $HOME/' + path)
        print "--- The folder for pose ---"
        print "OK"
    else:
        print "--- Write into this folder ---"

def extract(path, file_name):
    rospy.init_node('sensor_tf_listener', anonymous=True)

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)

    rate = rospy.Rate(10.0)
    flag = False
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform('base_link','odom', rospy.Time())
            #print(type(trans))
            f_path = os.environ['HOME'] + '/' + path + '/' + file_name
            f = open(f_path,'a')
            f.write(str(trans) + '\n')
            #f.write(str(rot) + '\n')
            flag = True
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rate.sleep()
            continue
        finally:
            if flag:
                f.close()
            else:
                rospy.logerr("no such tf!")
        rospy.sleep(1)
    rate.sleep()

file_name_for_read= raw_input('pose_stamps保存文件名:')
path = 'sensor_msgs_of_carto'
mkdir(path)
extract(path, file_name_for_read)
